/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: IO_Dummy.cpp,v 1.4 2009-10-22 14:35:53 morsko Exp $
 *
 * \file
 * \brief Dummy implementation for IO Server interface.
 *
 * This implementation can act as an example / codebase for any
 * SpeedCtrl server based module implementation. This code follows a
 * pseudogood coding style, and also provides a basic parameter
 * parser that can be directly used with slight modifications.
 * 
 * \todo 
 */
#include <stdio.h>
#include <stdlib.h>
#include "owndebug.h"
#include "IOServer.hpp"
#include "binbag.h"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
using namespace MaCI::IO;
//*****************************************************************************

void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tSpeedCtrl_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tUse 'name' as MaCIGroup for this service\n");
  fprintf(stderr,"  [-I name]\t\t\tUse 'name' as Instancename for this service\n");

  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

static void SetDigitalPin(const TDigitalPinState *aState)
{
  dPrint(1,"COMMAND: SET_DIGITAL_PIN (pin %d, state %d)",
	 aState->id,
	 aState->state);


}
//*****************************************************************************
     
static void SetDigitalMask(const TDigitalBlockMask *aState)
{
  dPrint(1,"COMMAND: SET_DIGITAL_MASK (id %d, state %d, mask %d)",
	 aState->state.id,
	 aState->state.state,
	 aState->mask);
}
//*****************************************************************************
      
static void SetAnalogVoltage(const TAnalogVoltage *aVoltage)
{
  dPrint(1,"COMMAND: SET_ANALOG_VOLTAGE (channel %d, voltage %f)",
	 aVoltage->channel,
	 aVoltage->voltage);

}
//*****************************************************************************

static TDigitalPinState GetDigitalPin(const TDigitalPinState *aState){

  dPrint(1,"COMMAND: GET_DIGITAL_PIN (id %d)",
	 aState->id);
  if(aState->id%2 == 1){
    dPrint(1,"REPLYING... state is 1");
    return TDigitalPinState(aState->id,1);
  }else{
    dPrint(1,"REPLYING... state is 0");
    return TDigitalPinState(aState->id,0);
  }
}
//*****************************************************************************
         
static TDigitalBlockState GetDigitalBlock(const TDigitalPinState *aState)
{
  dPrint(1,"COMMAND: GET_DIGITAL_BLOCK (id %d)",
	 aState->id);

  if(aState->id %2 ==1){
    dPrint(1,"REPLYING... state is 1010 (10 in decimal");
    return TDigitalBlockState(aState->id, 10); 
  }else{
    dPrint(1,"REPLYING... state is 1011 (11 in decimal");
    return TDigitalBlockState(aState->id, 11); 
  }

}
//*****************************************************************************
           
static TAnalogVoltage GetAnalogVoltage(const TAnalogVoltage *aVoltage)
{
  dPrint(1,"COMMAND: GET_ANALOG_VOLTAGE(channel %d)",
	 aVoltage->channel);

  float volt = 3.141592 + aVoltage->channel;
  dPrint(1,"REPLYING... voltage is %f",volt);

  return TAnalogVoltage(aVoltage->channel,volt);


}
//*****************************************************************************

int main(int argc, char *argv[])
{
 // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string maciGroupName;
  std::string interfaceInstanceName = "Dummy";

  // Objects
  bool run = true;
  gimi::GIMI g;
  MaCI::MaCICtrl::CMaCICtrlServer mcs(&g);
  MaCI::IO::CIOServer ioS(&mcs, 0); 

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(4);
  debugSetLogFilename("IO_Dummy.log");

  // Greet
  fprintf(stderr,
	  "\nIO_Dummy - MaCI IO Interface implementation for Dummy - v0.00001\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "I:G:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

    case 'G':
      maciGroupName = optarg;
      dPrint(3,"Using MaCIGroupName '%s'", optarg);
      break;

    case 'I':
      interfaceInstanceName = optarg;
      dPrint(3,"Using InterfaceInstance Name '%s'", optarg);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  mcs.SetGroupName(maciGroupName);
  ioS.SetInterfaceInstanceName(interfaceInstanceName);
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    MaCI::EMaCIError e;
    if ( (e = ioS.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open SpeedCtrlServer instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {
      ////// Execute test section ///////

      // Run as long as 'run' is set.
      while (run) {
	using namespace MaCI::IO;

	// Construct Data element
	CIOData iodata;
	TReplyInfo rInfo(0,0);
	gim::binbag::CBinBag bb;
	// Now, sit back and wait for command to arrive (for 1000ms)
        e = ioS.WaitCommand(iodata,rInfo, 1000);

	// Got command? (No timeout or error)
        if (e == MaCI::KMaCIOK) {
	  const TCommand *cmd = iodata.GetCommand();
	  assert(cmd != NULL);
	  // Switch based on command type
	  switch(cmd->cmd) {
	  case KCommandSetDigitalPin:{
	    iodata.Print(1);
	    const TDigitalPinState *state = iodata.GetDigitalPinState();
	    if(state){
	      SetDigitalPin(state);
	    } else {
	      dPrint(1,"WARNING: No digital pin  element available even SetDigitalPin was commanded!");
	    }
	    break;
	  }
	  case KCommandSetDigitalBlock:{
	    dPrint(1,"SetDigitalBlock");
	    iodata.Print(1);
	    const TDigitalBlockMask *bmask= iodata.GetDigitalBlockMask();
	    if(bmask){
	      SetDigitalMask(bmask);
	    }else{
	      dPrint(1,"WARNING: No digital block element available even SetDigitalMask was commanded!");
	    }
	    break;
	  }
	  case KCommandSetAnalogVoltage:{
	    const TAnalogVoltage *volt = iodata.GetAnalogVoltage();
	    if(volt){
	      SetAnalogVoltage(volt);
	    }else{
	      dPrint(1,"WARNING: No analog voltage element available even SetAnalogVoltage was commanded!");
	    }
	    break;
	  }
	  case KCommandGetDigitalPin:{
	    const TDigitalPinState *pstate  = iodata.GetDigitalPinState();
	    if(pstate){
	      const TDigitalPinState rState = GetDigitalPin(pstate);
	      CIOData ioDataReply;
	      ioDataReply.EncodeTo(&bb);
	      ioDataReply.SetCommand(TCommand(KCommandGetDigitalPin));
	      ioDataReply.SetDigitalPinState(rState);
	      e = ioS.SendReply(ioDataReply,rInfo);
	      if(e != MaCI::KMaCIOK){
		dPrint(1,"WARNING: Reply sending failed!");
	      }
	    }else{
	      dPrint(1,"WARNING: No digital pin element available even GetDigitalPin was commanded!");
	    }
	      
	    break;
	  }
	  case KCommandGetDigitalBlock:{
	    const TDigitalBlockState *bstate  = iodata.GetDigitalBlockState();
	    if(bstate){
	      const TDigitalBlockState rState = GetDigitalBlock(bstate);
	      CIOData ioDataReply;
	      ioDataReply.EncodeTo(&bb);
	      ioDataReply.SetCommand(TCommand(KCommandGetDigitalBlock));
	      ioDataReply.SetDigitalBlockState(rState);
	      e = ioS.SendReply(ioDataReply,rInfo);
	      if(e != MaCI::KMaCIOK){
		dPrint(1,"WARNING: Reply sending failed!");
	      }
	    }else{
	      dPrint(1,"WARNING: No digital mask element available even GetDigitalBlockState was commanded!");
	    }
	      
	    break;
	  }
	  case KCommandGetAnalogVoltage:{
	    const TAnalogVoltage *volt  = iodata.GetAnalogVoltage();
	    if(volt){
	      const TAnalogVoltage rState = GetAnalogVoltage(volt);
	      CIOData ioDataReply;
	      ioDataReply.EncodeTo(&bb);
	      ioDataReply.SetCommand(TCommand(KCommandGetAnalogVoltage));
	      ioDataReply.SetAnalogVoltage(rState);
	      e = ioS.SendReply(ioDataReply,rInfo);
	      if(e != MaCI::KMaCIOK){
		dPrint(1,"WARNING: Reply sending failed!");
	      }
	    }else{
	      dPrint(1,"WARNING: No analog voltage element available even GetAnalogVoltage was commanded!");
	    }
	      
	    break;
	  }
	  default:{
	    dPrint(1,"No implementation for command %d!", cmd->cmd);
	    break;
	  }
	  }

        } else if (e == MaCI::KMaCITimeout) {
          // No, timeout waiting for command. Print and wait again.
          dPrint(8,"Timeout waiting for Command");
                 
        } else {
          // Quck? Some error occured? Print it, sleep an extra second and try again.
          dPrint(1,"Failed to WaitCommand()! (%s)",
                 GetErrorStr(e).c_str());
          ownSleep_ms(1000);
        }
      
      }
    }
  }

  return 0;
} 

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
